import sys
sys.path.append('/home/pi/MasterPi')
import HiwonderSDK.Sonar as Sonar
import HiwonderSDK.Board as Board
import HiwonderSDK.mecanum as mecanum
import HiwonderSDK.Board as Board

chassis = mecanum.MecanumChassis()
HWSONAR = Sonar.Sonar()

# 初始位置
def initmove():
    chassis.set_velocity(0, 0, 0)  # 停止底盘运动

# 初始化调用
def init():
    print("Avoidance Init")
    initmove()


# 开始玩法调用
def start():
    global __isRunning, forward, stopMotor

    __isRunning = True
    forward = True
    stopMotor = True
    print("Avoidance Start")


# 停止玩法调用
def stop(signum, frame):
    global __isRunning

    __isRunning = False
    chassis.set_velocity(0, 0, 0)
    print("Avoidance Stop")

if __name__ == '__main__':
    init()
    start()
    try:
        Board.setMotor(1, 52)
        Board.setMotor(2, 40)
        Board.setMotor(3, 40)
        Board.setMotor(4, 40)
    except KeyboardInterrupt:
        chassis.set_velocity(0, 0, 0)
        HWSONAR.setPixelColor(0, Board.PixelColor(0, 0, 0))
        HWSONAR.setPixelColor(1, Board.PixelColor(0, 0, 0))
        print("Avoidance Exit")
